function result = run_fastslam(true_traj, obstacles, params)
% 运行FastSLAM算法
% 输入:
%   true_traj - 真实轨迹 [3 x steps]
%   obstacles - 障碍物位置 [N x 2]
%   params - 参数结构体
% 输出:
%   result - 结果结构体

    steps = size(true_traj, 2);
    dt = params.dt;
    Q = params.Q;
    R = params.R;
    laser_range = params.laser_range;
    laser_fov = params.laser_fov;
    num_beams = params.num_beams;
    laser_noise = params.laser_noise;
    
    % 进度显示选项
    if isfield(params, 'show_progress')
        show_progress = params.show_progress;
    else
        show_progress = false;
    end
    
    % FastSLAM参数
    N_particles = 50;
    resample_threshold = 0.5;
    initial_state = true_traj(:, 1);
    
    % 初始化粒子
    particles = initialize_particles(N_particles, initial_state, [0.1; 0.1; 0.02]);
    
    estimated_traj = zeros(3, steps);
    
    % 主循环
    progress_interval = max(1, round(steps/20));
    
    for t = 1:steps
        % 显示进度（估算路标数）
        if show_progress && (mod(t, progress_interval) == 0 || t == 1 || t == steps)
            percent = round(100 * t / steps);
            % 从最佳粒子估算路标数
            [~, best_idx] = max([particles.weight]);
            n_landmarks = length(particles(best_idx).landmarks);
            fprintf('  [%3d%%] 步骤 %4d/%4d | 粒子路标数: %3d\r', ...
                    percent, t, steps, n_landmarks);
            if t == steps
                fprintf('\n');
            end
        end
        
        % 计算控制输入
        if t == 1
            control = [0; 0];
        else
            dx = true_traj(1, t) - true_traj(1, t-1);
            dy = true_traj(2, t) - true_traj(2, t-1);
            dtheta = wrapToPi(true_traj(3, t) - true_traj(3, t-1));
            control = [sqrt(dx^2 + dy^2) / dt; dtheta / dt];
        end
        
        % 激光观测
        [observations, observed_ids] = observation_model_laser(true_traj(:, t), obstacles, ...
                                        laser_range, laser_fov, num_beams, laser_noise);
        
        % 预测
        particles = particle_predict(particles, control, dt, Q);
        
        % 更新
        particles = particle_update(particles, observations, observed_ids, R);
        
        % 重采样
        N_eff = 1 / sum([particles.weight].^2);
        if N_eff < N_particles * resample_threshold
            particles = resample_particles(particles);
        end
        
        % 估计状态
        estimated_traj(:, t) = estimate_state(particles);
    end
    
    % 构建结果
    result = struct();
    result.trajectory = estimated_traj;
    result.particles = particles;
    
    % 统计路标
    all_landmarks = [];
    for i = 1:length(particles)
        if particles(i).weight > 0.01
            all_landmarks = [all_landmarks; particles(i).landmark_ids];
        end
    end
    result.discovered_landmarks = unique(all_landmarks);
    result.n_landmarks = length(result.discovered_landmarks);
end
